#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "package.h"
#include "usart.h"
#include "tick.h"

#undef TRACE_LEVEL
#undef TRACE_TITLE
#undef TRACE_TITLE
#define TRACE_LEVEL	TRACE_LEVEL_NO_TRACE
#define TRACE_NAME  TRACE_NAME_DISABLE
#define TRACE_TITLE "PKG"
#include "trace.h"

// package state
typedef enum {
	PACKAGE_NEED_SYNC,
	PACKAGE_NEED_LENGTH,
	PACKAGE_READ_LENGTH,
} PackageReadingState_t;

/* Package SYNC header */
#define SYNC_HEADER0		(char)85		// U
#define SYNC_HEADER1		(char)79		// O

#define READ_TIMEOUT_MS 	200

#define PKG_OUTPUT_CONSOLE	0

// package sync header
const static char SYNC[2] = {SYNC_HEADER0, SYNC_HEADER1};
// const static uint16_t SYNC_WORD = ((SYNC_HEADER0 << 8) | SYNC_HEADER1);

static void (*PackageHanlder)(SensorXPackage_t *package) = NULL;

// check the package timeout event
static int isPackageTimeout(uint32_t PkgTimeMark) {
	if ( (get_tick_count() - PkgTimeMark) > READ_TIMEOUT_MS) {
		TRACE_DEBUG("PKG TIMEOUT");
		return 1;
	} else {
		return 0;
	}
}

void RegisterPackageHandler(void (*fn)(SensorXPackage_t *)) {
	if(fn) {
		PackageHanlder = fn;
	}
}

void UnRegisterPackageHandler(void) {
	PackageHanlder = NULL;
}

static void WriteUSARTBuffered(const char *buffer, int len) {	
	static char cache[64];
	static int count = 0;
	
	if(buffer == NULL) {
		USART_Write(USART, cache, count);
		count = 0;
	} else {
		memcpy(&cache[count], buffer, len);
		count += len;
	}
}

void WritePackage(PKG_CLASS command, uint32_t time, uint16 *buffer, uint8_t bufferLen) {
	int i;
	uint16_t checkSum = command;
	uint16_t timeLSB;
	uint16_t timeMSB;
	char packageLen;
	
	// command + timeLSB + timeMSB + checksum = 4
	packageLen = bufferLen + 4;
	timeLSB = time & 0xFFFF;
	timeMSB = (time >> 16) & 0xFFFF;
	
	checkSum += timeLSB;
	checkSum += timeMSB;
	for(i = 0; i < bufferLen; i ++) {
		checkSum += buffer[i];
	}
	
	TRACE_DEBUG("OUT PKG CMD=0x%x", (uint16_t)command);
	TRACE_DEBUG("LEN=%d: ", packageLen);
	for(i = 0; i < bufferLen; i ++) {
		TRACE_DEBUG_WP("0x%x ", buffer[i]);
	}
	TRACE_DEBUG("sum=0x%x", checkSum);
	
	/* SYNC Header */
	// USART_Write(USART, (const char *)SYNC, 2);
	WriteUSARTBuffered(SYNC, 2);
	
	/* Package length in WORD */
	// USART_Write(USART, (const char *)&packageLen, 1);
	WriteUSARTBuffered((const char *)&packageLen, 1);
	
	/* command */
	// USART_Write(USART, (const char *)&command, 2);
	WriteUSARTBuffered((const char *)&command, 2);
	
	/* time mark */
	// USART_Write(USART, (const char *)&timeLSB, 2);
	// USART_Write(USART, (const char *)&timeMSB, 2);
	WriteUSARTBuffered((const char *)&timeLSB, 2);
	WriteUSARTBuffered((const char *)&timeMSB, 2);
	
	/* data */
	for(i = 0; i < bufferLen; i ++) {
		// USART_Write(USART, (const char *)&buffer[i], 2);
		WriteUSARTBuffered((const char *)&buffer[i], 2);
	}
	/* check sum */
	// USART_Write(USART, (const char *)&checkSum, 2);
	WriteUSARTBuffered((const char *)&checkSum, 2);
	
	// Flush the data
	WriteUSARTBuffered(NULL, 0);
}

void WriteCmdResponse(PKG_CLASS cmd_value, int result, uint16_t *_buffer, uint8_t bufferLength) {
	uint16_t buffer[16];
	
	if(_buffer == NULL) {
		bufferLength = 0;
	}
	
	bufferLength += 2;
	buffer[0] = cmd_value;
	if (result == 0) {
		buffer[1] = 0;
	} else {
		buffer[1] = 1;
	}
	
	WritePackage(PKG_COMMAND_RESULT, get_tick_count(), buffer, bufferLength);
}

/* UART Package Listen */
int PackageListener(void) {
	static SensorXPackage_t input;
	static uint32_t PkgTimeMark = 0;
	static int syncIndex = 0;
	static PackageReadingState_t state;
	static int length;
	static uint16_t datBuffer[16];
	static char *ptr;
	int n;
	char buffer;
	
	n = USART_Read(USART, &buffer, sizeof(buffer));
	if ((state != PACKAGE_NEED_SYNC) && isPackageTimeout(PkgTimeMark)) {
		state = PACKAGE_NEED_SYNC;
		return -1;
	}
	if (n) {
		switch(state) {
		case PACKAGE_NEED_SYNC:
			if (buffer == SYNC[syncIndex]) {
				syncIndex ++;
			} else {
				syncIndex = 0;
			}
			if (syncIndex >= 2) {
				TRACE_DEBUG("IN SYNC DONE");
				syncIndex = 0;
				PkgTimeMark = get_tick_count();
				state = PACKAGE_NEED_LENGTH;
			}
		break;
		case PACKAGE_NEED_LENGTH:
			if ( (buffer < 0) || (buffer > 127)) {
				TRACE_ERROR("IN PKG LEN ERROR");
				state = PACKAGE_NEED_SYNC;
			} else {
				TRACE_DEBUG("IN PKG LEN = %d", buffer);
				input.pkgLengthInWORD = buffer;
				length = input.pkgLengthInWORD * sizeof(uint16_t);
				ptr = (char *)datBuffer;
				state = PACKAGE_READ_LENGTH;
			}
		break;
		case PACKAGE_READ_LENGTH:
			if (length) {
				*(ptr ++) = buffer;
				length --;
			}
			/* Command Package is ready */
			if (length == 0) {
				TRACE_DEBUG("IN READ DONE");
				input.command = datBuffer[0];
				input.checkSum = datBuffer[input.pkgLengthInWORD - 1];
				input.buffer = &datBuffer[1];
				if (PackageHanlder)
					PackageHanlder(&input);
				state = PACKAGE_NEED_SYNC;
			}
		break;
		default:
			state = PACKAGE_NEED_SYNC;
		}
	}
	return 0;
}

